Nonlinear factor recovery for long-term SLAM

For long-term operations, graph-based SLAM approaches require to marginalize nodes in order to control the computational cost. In this paper, we present a method to recover a set of nonlinear factors that best represents the marginal distribution in terms of Kullback-Leibler divergence. The proposed method, which we call nonlinear factor recovery (NFR), estimates both the mean and the information matrix of the set of nonlinear factors, where the recovery of the latter is equivalent to solving a convex optimization problem. NFR is able to provide either the dense distribution or a sparse approximation of it. In contrast to previous algorithms, our method does not necessarily require a global linearization point and can be used with any nonlinear measurement function. Moreover, we are not restricted to only use tree-based sparse approximations and binary factors, but we can include any topology and correlations between measurements. Experiments performed on several publicly available datasets demonstrate that our method outperforms the state of the art with respect to the Kullback-Leibler divergence and the sparsity of the solution.